using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.FEZ;

namespace FEZ_Cobra_Console_Application1
{
    /// <summary>
    /// 15 ms period
    /// </summary>
    public class Motors
    {
        PWM front;
        OutputPort ENA;
        OutputPort IN1;
        OutputPort IN2;
        OutputPort IN3;
        OutputPort IN4;
        OutputPort ENB;

        static Motors m;
        private int speed;
        public bool isGoingForward = false;

        private Motors()
        {
            
            IN1 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.IO15, false);
            IN2 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.IO18, false);
            ENA = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.IO16, false);

            front = new PWM((PWM.Pin)FEZ_Pin.PWM.PWM0);
            speed = 0;

            this.turn_Straight();
        }

        /// <summary>
        /// Singleton class
        /// </summary>
        /// <returns></returns>
        public static Motors getMotor()
        {
            if (m == null)
                m = new Motors();
            return m;
        }

        /// <summary>
        /// 2
        /// </summary>
        public void turn_Right()
        {
            Debug.Print("Turn Right");
            front.SetPulse(15000000, 2000000);
        }

        /// <summary>
        /// 1.5 ms
        /// </summary>
        public void turn_Straight()
        {
            Debug.Print("Turn Straight");
            front.SetPulse(15000000, 1500000);
        }

        /// <summary>
        /// 1
        /// </summary>
        public void turn_Left()
        {
            Debug.Print("Turn Left");
            front.SetPulse(15000000, 1000000);
        }

        int cntd = 0;
        public void forwards()
        {
            Debug.Print("Move Forwards " + cntd);
            cntd++;
            IN1.Write(true);
            IN2.Write(false);
            ENA.Write(true);
            isGoingForward = true;
        }


        public void break_now()
        {
            Debug.Print("Break Now");
            IN1.Write(false);
            IN2.Write(false);
            ENA.Write(true);    //may change
            isGoingForward = false;
        }

        public void backwards()
        {
            Debug.Print("Move Backwards");
            IN1.Write(false);
            IN2.Write(true);
            ENA.Write(true);
            isGoingForward = false;
        }

        public void setSpeed(int inSpeed)
        {
            speed = inSpeed;
        }

    }
}
